#include <stdio.h>

#define _USE_MATH_DEFINES
#include <math.h>
#include <stdlib.h>
#include <complex.h>
#include "fftw3.h"
#include "dpi_algo.h"
#include "pub_lib.h"

//=============================================================================
/*
 * usage:
 *	  float *image;
 *    fftwf_complex *out;
 *    void* p = fft_init(w, h, &image, &out);
 *
 *    Initialize input signal: fill image[]
 *    FFT: fft_r2c(p, image, out);
 *    IFFT: ifft_c2r(p, out, image);
 *
 *    fft_exit(p);
 *
 *  注意：
 *	  只要宽度和高度不变，就可以无限的用这个 plan
 *      fft_init返回的指针p，只和宽度和高度有关。
 *      所有 fft 处理的数据，用以下4个函数来申请：
 *      fftwf_alloc_real, fftwf_alloc_complex
 *      fftwf_malloc, fftwf_free
 *      这些函数分配的内存保证是对齐的
 *
 *    如果宽度或高度变了，则可以通过调用
 *    fft_init 产生新的计划，只需保留计划指针即可。
 * */

typedef struct{
	fftwf_plan p; //forward
	fftwf_plan ip; //backward

	float *in;
	fftwf_complex *out;
}plan_fft;

/*
 * 初始化FFT
 * in: 要处理的图片的宽w 和高h
 *
 * out:
 *	float image[h][w]
 *	fftwf_complex out[h][w/2+1]
 *
 * 返回指向计划的指针
 * */
void* fft_init(int w, int h, float** image_data, fftwf_complex** out_data)
{
	int n1_cplx = w / 2 + 1;
	float *in = fftwf_alloc_real(w * h);
	fftwf_complex *out = fftwf_alloc_complex(h * n1_cplx);

	plan_fft* ps = malloc(sizeof(plan_fft));
	fftwf_plan p  = fftwf_plan_dft_r2c_2d(h, w, in, out, FFTW_ESTIMATE);
	fftwf_plan ip = fftwf_plan_dft_c2r_2d(h, w, out, in, FFTW_ESTIMATE);
	ps->p = p;
	ps->ip = ip;
	ps->in = in;
	ps->out = out;

	*image_data = in;
	*out_data = out;

	return (void*)ps;
}


/**
 * internal use
 * not free in, out
 */
static void fft_exit_save(void* p)
{
	plan_fft* ps = (plan_fft*)p;
	fftwf_destroy_plan(ps->p);
	fftwf_destroy_plan(ps->ip);
	free(ps);
}

//free all resources
void fft_exit(void* p)
{
	plan_fft* ps = (plan_fft*)p;

	fftwf_free(ps->in);
	fftwf_free(ps->out);

	fft_exit_save(p);
}


/*
 * 对一个2D 图片进行逆傅里叶变换
 * fft_out: 就是 FFT 的结果，可以经过一些滤波处理，长度是 (w/2 + 1) * h 的复数
 * image: float image[h][w]
 * 图片宽 w，高 h
 *
 * 为什么处理后的结果，宽度只有 w/2+1
 * 每一行的宽度为w，本来应该计算出 w 个复数
 * 数据是对称的（共轭的）
 *
 * X(k) 表示输出的某一行的结果
 * 图片宽 w，高 h
 * X(row, col) = conj(X(h - row, w - col))
 * */
//2D FFT, read to complex
void fft_r2c(void* p, float *in, fftwf_complex *out)
{
	plan_fft* ps = (plan_fft*)p;
	fftwf_execute_dft_r2c(ps->p, in, out);
}

//2D IFFT: complex to real
void ifft_c2r(void* p, fftwf_complex *in, float *out)
{
	plan_fft* ps = (plan_fft*)p;
	fftwf_execute_dft_c2r(ps->ip, in, out);
}

/*
 * 2D FFT: c2c
 * h, w: image size
 * in[h][w]
 * out[h][w]
 */
void fft_c2c(fftwf_complex* in, fftwf_complex* out, int h, int w)
{
	fftwf_plan plan;
	plan = fftwf_plan_dft_2d(h, w, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftwf_execute(plan);
	fftwf_destroy_plan(plan);
}

/*
 * 2D IFFT: c2c
 * h, w: image size
 * in[h][w]
 * out[h][w]
 */
void ifft_c2c(fftwf_complex* in, fftwf_complex* out, int h, int w)
{
	fftwf_plan plan;
	plan = fftwf_plan_dft_2d(h, w, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
	fftwf_execute(plan);
	fftwf_destroy_plan(plan);
}

///////////////////////////////////////////////////////////////////////////////

/*
 * 得到 data[index] 的值
 * index: 0 to w*h-1, 当前复数值在 w*h 数组中的索引
 * val: 当前的复数值
 * conj: 0, original; 1: conj
 * pri: in-out 参数
 * */
typedef void (*fft_result_handler)(int index, fftwf_complex val, int conj, void* pri);

/*
 * traverse FFT output
 * in: FFT 结果，fftwf_complex* data[h][w/2+1]
 * */
void calc_fft_common(fftwf_complex* data, int w, int h,\
	fft_result_handler fft_handler, void* pri)
{
	int i, j, index, halfi, index_sym;
	int cplx = w/2 + 1;

	int left;
	for(j = 0; j < h; j++) {
		for(i = 0; i < cplx; i++) {
			index = j * w + i;
			halfi = j * cplx + i;
			fft_handler(index, data[halfi], 0, pri);

			/* (j,i) to (h-j,w-i) 共轭对
			 * [0 - cplx): 已经处理
			 * [cplx, w - 1]:尚未处理
			 * 上面的范围用 w 减
			 * left = w - cplx
			 * w - (w-1) = 1， 所以i 的范围是 [1, left]
			 * */
			left = w - cplx;
			if((i >= 1) && (i <= left)) {
				if(j == 0) //第一行特殊处理，和本行对称, h - 0 越界
					index_sym = w - i;
				else
					index_sym = (h - j) * w + (w - i); //(j,i) to (h-j,w-i)
				fft_handler(index_sym, data[halfi], 1, pri);
			}
		}
	}
}

void fft_spectrum_handler(int index, fftwf_complex val, int conj, void* pri)
{
	float *spectrum;
	spectrum = (float*)pri;
	spectrum[index] = cabsf(val);
}

/* 计算频谱
 * 把FFT的输出结果，计算频谱后，结果存在 s[]
 * float s[h][w]
 * */
void calc_fft_spectrum(fftwf_complex* data, float* s, int w, int h)
{
	calc_fft_common(data, w, h, fft_spectrum_handler, (void*)s);
}

void fft_angle_handler(int index, fftwf_complex val, int conj, void* pri)
{
	float *angles;
	fftwf_complex a;
	angles = (float*)pri;
	if(conj)
		a = conjf(val);
	else
		a = val;
	angles[index] = cargf(a);
}

/*
 * in: FFT 结果，fftwf_complex* data[h][w/2+1]
 * out: float angles[h][w]
 * */
void calc_fft_angle(fftwf_complex* data, float *angles, int w, int h)
{
	calc_fft_common(data, w, h, fft_angle_handler, (void*)angles);
}

void fft_fill_handler(int index, fftwf_complex val, int conj, void* pri)
{
	fftwf_complex *result = (fftwf_complex*)pri;
	if(conj)
		result[index] = conjf(val);
	else
		result[index] = val;
}

/*
 * 把 FFT 的输出结果输出到res 中。 fftwf_complex res[h][w]
 * in: FFT 结果，fftwf_complex* data[h][w/2+1]
 * out: fftwf_complex res[h][w]
 *
 * e.g.
 *  result = malloc(w * h * sizeof(fftwf_complex));
 *  fill_fft_result(data, result, w, h);
 * */
void fill_fft_result(fftwf_complex* data, fftwf_complex* res, int w, int h)
{
	calc_fft_common(data, w, h, fft_fill_handler, (void*)res);
}

/*
 * 从相角数据重建一副图
 * 频谱 = 1
 * in:
 *  p: reuse the fft plan
 *	float angles[h][w]，图片的相角数据
 *	w, h: image width, height
 *
 * out: float res[h][w] 图片
 * */
void fft_build_from_angle(void *p, float *angles, float *res, int w, int h)
{
	int cplx = w/2 + 1;
	fftwf_complex *out = fftwf_alloc_complex(h * cplx);

	int i, j, index, id;
	float alpha;
	for(j = 0; j < h; j++) {
		for(i = 0; i < cplx; i++) {
			index = j * cplx + i; //out[h][cplx]
			id = j * w + i; //angles[h][w]
			alpha = angles[id];
			out[index] = build_complex(cosf(alpha), sinf(alpha));
		}
	}
	ifft_c2r(p, out, res);

	fftwf_free(out);
}

/*
 *  * P157, fomula 4.6-26
 * spatial domain: p >= A + B - 1
 * 为了方便FFT计算，p 还需要是偶数 even
 * Avoid wraparound error
 * 可以在空间域避免缠绕错误
 *
 * 在频率域，这样做可以消除振铃错误
 * Avoid ringing error
 *
 * in: 滤波器的宽高mask_w，mask_h, 图片的宽高 w, h
 * out: 新的宽高
 * */
void filter_freq_newsize(int mask_w, int mask_h, int w, int h,\
	int* P, int* Q)
{
	int neww, newh;
	neww = mask_w + w - 1;
	if(neww & 1) //should be even
		neww += 1;

	newh = mask_h + h - 1;
	if(newh & 1)
		newh += 1;
	*P = neww;
	*Q = newh;
}

/*
* real = 1, set real part = 0
* real = 0, set imaginary part = 0
*/
void set_complex_zero(fftwf_complex* out, int w, int h, int real)
{
	int i, j, index;
	int cplx = w/2 + 1; 
	fftwf_complex v;

	for (j = 0; j < h; j++) {
		for (i = 0; i < cplx; i++) {
			index = j * cplx + i;
			v = out[index];
			if (real) {
				out[index] = build_complex(0, cimagf(v));
			}
			else {
				out[index] = build_complex(crealf(v), 0);
			}
		}
	}
}

//to center
static void fft_shift(float* fp, int w, int h)
{
	//fp  * (-1)^(x + y)
	int x, y, index;
	for (y = 0; y < h; y++) {
		for (x = 0; x < w; x++) {
			index = y * w + x;
			if ((x + y + 1) & 1)
				fp[index] = -fp[index];
		}
	}
}

/*
 * 空间滤波器 spatial 变换到频域滤波器 out
 * in:
 *	原始的空间滤波器 spatial[h][w]
 *  频域滤波器的宽P, 高Q
 *	原始空间滤波器的奇偶性 attr
 *
 * out: 频率域滤波器
 * 用完之后，由用户调用 fftwf_free 释放内存
 * */
fftwf_complex* spatial_filter_to_freq(char* spatial, int w, int h,
	int P, int Q, filter_sym_attr attr)
{
	float* filter;
	fftwf_complex* out; //filter in frequency domain
	void* plan;
	plan = fft_init(P, Q, &filter, &out);

	//padding with zero, spatial[h][w] to filer[Q][P]
	fill_zero_char(filter, P, Q, spatial, w, h);

	//step1: center
	fft_shift(filter, P, Q);

	//step2: DFT
	fft_r2c(plan, filter, out);

	//step3: handle result
	if(attr == SYM_ATTR_ODD) {
		//set real part to zero
		set_complex_zero(out, w, h, 1);
	} else if(attr == SYM_ATTR_EVEN) {
		//set imaginary part to to zero
		set_complex_zero(out, w, h, 0);
	}

	fftwf_free(filter);
	fft_exit_save(plan);
	return out;
}

//b = a * b
void fft_cmulti(fftwf_complex* a, fftwf_complex* b, int w, int h)
{
	int i, j, t;
	int cplx = w/2 + 1;
	float real, img;

	for(j = 0; j < h; j++) {
		for(i = 0; i < cplx; i++) {
			t= j * cplx + i;
			real = crealf(a[t]) * crealf(b[t]) - cimagf(a[t]) * cimagf(b[t]);
			img  = crealf(a[t]) * cimagf(b[t]) + cimagf(a[t]) * crealf(b[t]);
			b[t] = build_complex(real, img);
		}
	}
}

//s[h][w] = filter * data
void fft_rrmulti(fftwf_complex* filter, unsigned char* data, float* s,
	int w, int h)
{
	int i, total;
	total = h * w;

	for(i = 0; i < total; i++) {
		s[i] = crealf(filter[i]) * data[i];
	}
}

/*
 * 在频率域中滤波
 * image[h][w]: 要滤波的图像
 * filter[Q][P]: 频域滤波器, P, Q：滤波器的尺寸
 *
 * out[h][w]：滤波结果放在这里
 * */
void filter_freq_domain(unsigned char* image, int w, int h,
	fftwf_complex* filter, int P, int Q, float* out)
{
	void* plan;
	float* fp;
	fftwf_complex* fft_out;
	plan = fft_init(P, Q, &fp, &fft_out);

	//total 7 steps
	//1. cal P, Q, already finished
	//2. fill zero
	fill_zero_byte(fp, P, Q, image, w, h);

	//3. fp  * (-1)^(x + y)
	fft_shift(fp, P, Q);
	//4. FFT
	fft_r2c(plan, fp, fft_out);

	//5. filter in frequency domain: H(u,v) * F(u,v)
	fft_cmulti(filter, fft_out, P, Q);

	//6. IFFT
	ifft_c2r(plan, fft_out, fp);
	//de-center, image(x,y) * (-1) ^ (x+y)
	fft_shift(fp, P, Q);

	//7. Crop to original size
	//fp[Q][P] to out[h][w]
	restore_ori_image(fp, P, Q, out, w, h);

	fft_exit(plan);
}


/*
 * LowPass Filter
 * r: D0
 * p = w, q = h
 * n: Butterworth order
 *
 * out: filter at frequency domain
 * user call fftwf_free(out) to free resources after finished filtering
 */
fftwf_complex* lpfilter(float r, int p, int q, freq_filter_type type, int n)
{
	int cplx = p/2 + 1;
	int ph = p/2;
	int qh = q/2;
	int i, j, index;
	float dis, rr, temp;

	rr = r * r;
	unsigned int max = ph * ph + qh * qh;
	fftwf_complex *out = fftwf_alloc_complex(cplx * q);
	for(j = 0; j < q; j++) {
		for(i = 0; i < cplx; i++) {
			index = j * cplx + i;
			dis = (i - ph) * (i - ph) + (j - qh) * (j - qh);
			switch(type) {
				case FREQ_FILTER_IDEAL:
					if(dis < rr)
						out[index] = build_complex(1, 0);
					else
						out[index] = build_complex(0, 0);
					break;
				case FREQ_FILTER_BTW:
					temp = powf(dis/rr, n);
					out[index] = build_complex(1.0f/(1.0f + temp), 0);
					break;
				case FREQ_FILTER_GAUSSIAN:
					temp = dis/ rr / (-2.0f);
					out[index] = build_complex(expf(temp), 0);
					break;
				case FREQ_FILTER_LAPlACE:
					//lp = 1 +       4 * pi * pi * dis * dis
					//hp = 1 - lp = -4 * pi * pi * dis * dis
					//temp = 4.0f * M_PI * M_PI * dis + 1.0f;
					temp = dis / max + 1.0f;
					out[index] = build_complex(temp, 0);
					break;
				default:
					break;
			}
		}
	}

	return out;
}

/*
 * HighPass Filter
 * r: D0
 * p = w, q = h
 * n: Butterworth order
 *
 * out: filter at frequency domain
 * user call fftwf_free(out) to free resources after finished filtering
 */
fftwf_complex* hpfilter(float r, int p, int q, freq_filter_type type, int n)
{
	fftwf_complex* out;
	out = lpfilter(r, p, q, type, n);

	int cplx = p/2 + 1;
	int i, total;
	float temp;
	total = cplx * q;
	for(i = 0; i < total; i++) {
		temp = 1.0f - crealf(out[i]);
		out[i] = build_complex(temp, 0);
	}

	return out;
}

//filter[] = k1 + k2 * filter[] 
void hpfilter_highboost(fftwf_complex* filter, float k1, float k2, int p, int q)
{
	int cplx = p / 2 + 1;
	int total = q * cplx;
	int i;
	float temp;
	for (i = 0; i < total; i++) {
		temp = k1 + k2 * crealf(filter[i]);
		filter[i] = build_complex(temp, 0);
	}
}

/*
 * 4.10 BandReject filter, BR
 * r: D0
 * w: Width of band
 * p, q: image width and height
 * n: Butterworth order
 *
 * out: filter at frequency domain
 * user call fftwf_free(out) to free resources after finished filtering
 */
fftwf_complex* brfilter(float r, float w, int p, int q, freq_filter_type type, int n)
{
	int cplx = p/2 + 1;
	int ph = p/2;
	int qh = q/2;
	int i, j, index;
	float dis, rr, temp, rp, rm, ww;

	rr = r * r;
	rp = (r + w/2) * (r + w/2);
	rm = (r - w/2) * (r - w/2);
	ww = w * w;
	fftwf_complex *out = fftwf_alloc_complex(cplx * q);

	float numerator, denom;
	for(j = 0; j < q; j++) {
		for(i = 0; i < cplx; i++) {
			index = j * cplx + i;
			dis = (i - ph) * (i - ph) + (j - qh) * (j - qh);
			switch(type) {
				case FREQ_FILTER_IDEAL:
					//(r - w/2)^2 <= dis <= (r + w/2)^2
					if(dis >= rm && dis <= rp)
						out[index] = build_complex(0, 0);
					else
						out[index] = build_complex(1, 0);
					break;
				case FREQ_FILTER_BTW:
					numerator = dis * ww;
					denom = (dis - rr) * (dis - rr) + MIN_NUMBER;
					temp = powf(numerator/denom, n);
					out[index] = build_complex(1.0f/(1.0f + temp), 0);
					break;
				case FREQ_FILTER_GAUSSIAN:
					numerator = (dis - rr) * (dis - rr);
					denom = dis * rr + MIN_NUMBER;
					temp = expf(numerator/denom);
					out[index] = build_complex(1.0f - temp, 0);
					break;
				default:
					break;
			}
		}
	}

	return out;
}

/*
 * Band Pass: BP filter
 * r: D0
 * w: Width of band
 * p, q: image width and height
 * n: Butterworth order
 *
 * out: filter at frequency domain
 * user call fftwf_free(out) to free resources after finished filtering
 */
fftwf_complex* bpfilter(float r, float w, int p, int q, freq_filter_type type, int n)
{
	fftwf_complex* out;
	out = brfilter(r, w, p, q, type, n);

	int cplx = p/2 + 1;
	int i, total;
	float temp;
	total = cplx * q;
	for(i = 0; i < total; i++) {
		temp = 1.0f - crealf(out[i]);
		out[i] = build_complex(temp, 0);
	}

	return out;
}

/*
 * 之前的坐标，是根据原图测量的
 * 对图像滤波时，原图放大2倍，所以
 * 相应坐标需要放大2倍
 *
 * in:
 *   center: 原图上测量的两点中心坐标，相对于原图左上角
 *   scale: default is 2.0
 *
 * out:
 *   新坐标
 *   用完后需要调用 free_center() 释放内存
 * */
notch_center* change_center(notch_center* center, float scale)
{
	notch_center* out = malloc(sizeof(notch_center));
	out->c = malloc(center->k * sizeof(ncenter));

	int i;
	out->k = center->k;
	for(i = 0; i < out->k; i++) {
		out->c[i].x = center->c[i].x * scale;
		out->c[i].y = center->c[i].y * scale;
	}
	return out;
}

void free_center(notch_center* center)
{
	free(center->c);
	free(center);
}


//flag = 1, dkk = D(k) * D(k)
//flag = 0, dkkp = D(-k) * D(-k)
static float calc_nr_dk(int u, int v, int M, int N, float uk, float vk, int flag)
{
	float a, b;
	if(flag) {
		a = u - uk;
		b = v - vk;
	} else {
		a = u - (M - uk);
		b = v - (N - vk);
	}
	return (hypotf(a, b) + MIN_NUMBER); //avoid div zero
}

/*
 * 4.10 Notch Reject filter, NR
 * r: D0, cutoff frequency
 * c: center pairs
 * p, q: image width and height
 * n: Butterworth order
 * full_size: if full size filter
 *
 * out: filter at frequency domain
 * user call fftwf_free(out) to free resources after finished filtering
 */
fftwf_complex* nrfilter(float r, notch_center* c, int p, int q,
	freq_filter_type type, int n, int full_size)
{
	int cplx = p/2 + 1;
	int i, j, k, index;
	float dkk, dkkp;

	//在进行FFT验证时，需要全尺寸滤波器，大小和原图像一样
	if(full_size)
		cplx = p;

	fftwf_complex *out = fftwf_alloc_complex(cplx * q);
	float a, b, result;
	for(j = 0; j < q; j++) {
		for(i = 0; i < cplx; i++) {
			index = j * cplx + i;
			switch(type) {
				case FREQ_FILTER_IDEAL:
					break;
				case FREQ_FILTER_BTW:
					result = 1.0f;
					for(k = 0; k < c->k; k++) {
						//dkk = Dk * Dk
						dkk  = calc_nr_dk(i, j, p, q, c->c[k].x, c->c[k].y, 1);
						dkkp = calc_nr_dk(i, j, p, q, c->c[k].x, c->c[k].y, 0);
						a = powf(r/dkk, 2*n);
						a = 1.0f / (1.0f + a);
						b = powf(r/dkkp, 2*n);
						b = 1.0f / (1.0f + b);
						result = result * a * b;
					}
					out[index] = build_complex(result, 0);
					break;
				case FREQ_FILTER_GAUSSIAN:
					break;
				default:
					break;
			}
		}
	}

	return out;
}

//Notch Pass: NP filter
fftwf_complex* npfilter(float r, notch_center* c, int p, int q,
	freq_filter_type type, int n, int full_size)

{
	fftwf_complex* out;
	out = nrfilter(r, c, p, q, type, n, full_size);

	int cplx = p/2 + 1;
	int i, total;
	float temp;

	if(full_size)
		cplx = p;

	total = cplx * q;
	for(i = 0; i < total; i++) {
		temp = 1.0f - crealf(out[i]);
		out[i] = build_complex(temp, 0);
	}

	return out;
}

static void recnotch_hor(fftwf_complex* out, int w, int h, int cy, int r, int lx, int rx)
{
	int i, j, index;
	for(j = cy - r; j <= (cy + r); j++) {
		for(i = 0; i < w; i++) {
			index = j * w + i;
			if(i < lx || i > rx)
				out[index] = build_complex(0, 0);
		}
	}
}

static void recnotch_ver(fftwf_complex* out, int w, int h, int cx, int r, int uy, int dy)
{
	int i, j, index;
	for(i = cx - r; i <= (cx + r); i++) {
		for(j = 0; j < h; j++) {
			index = j * w + i;
			if(j < uy || j > dy)
				out[index] = build_complex(0, 0);
		}
	}
}

/*
 * Generates rectangular notch (axes) filters.
%	H = recnotch(type, mode, P, Q, W, SV, SH) generates an P-by-Q
%	notch filter consisting of symmetric pairs of rectangles of
%	width W placed on the vertical and horizontal axes of the
%	(centered) frequency rectangle. The vertical rectangles start at
%	+SV and -SV on the vertical axis and extend to both ends of the
%	axis. Horizontal rectangles similarly start at +SH and -SH and
%	extend to both ends of the axis. These values are with respect
%	to the origin of the axes of the centered frequency rectangle.
%	For example, specifying SV = 50 creates a rectangle of width W
%	that starts 50 pixels above the center of the vertical axis and
%	extends up to the first row of the filter. A similar rectangle
%	is created starting 50 pixels below the center and extending to
%	the last row. W must be an odd number to preserve the symmetry
%	of the filtered Fourier transform.
%
%       Valid values of type are:
%
%           1: 'reject'    Notchreject filter.
%
%           0: 'pass'      Notchpass filter.
%
%
%       Valid values of mode are:
%
%           'both'          Filtering on both axes.
%
%           'horizontal'    Filtering on horizontal axis only.
%
%           'vertical'      Filtering on vertical axis only.
%
%       One of these three values must be specified in the call.
%  H = recnorch(type, both, M, N) sets
	W = start_offset, and SV = SH = start_offset.
 * */
fftwf_complex* recnorch(int type, enum recnotch_mode mode, int p, int q,
	int w, int sv, int sh)
{
	int cplx = p/2 + 1;
	int i, size;
	size = cplx * q;

	fftwf_complex *out = fftwf_alloc_complex(size);
	//all pass
	for(i = 0; i < size; i++)
		out[i] = build_complex(1, 0);

	int start_offset = 5;
	if(!w)
		w = start_offset;
	if(!sv)
		sv = start_offset;
	if(!sh)
		sh = start_offset;
	int cx, cy; //center
	cx = (p + 1) / 2;
	cy = (q + 1) / 2;

	//left, right, up, down
	int lx, rx, uy, dy;
	lx = cx - sh;
	rx = cx + sh;
	uy = cy - sv;
	dy = cy + sv;

	int r = w / 2;
	//Notchreject filter.
	if(mode == RECNOTCH_MODE_HOR){
		recnotch_hor(out, cplx, q, cy, r, lx, rx);
	} else if(mode == RECNOTCH_MODE_VER) {
		recnotch_ver(out, cplx, q, cx, r, uy, dy);
	} else {
		//both
		recnotch_hor(out, cplx, p, cy, r, lx, rx);
		recnotch_ver(out, cplx, q, cx, r, uy, dy);
	}

	if(!type) {
		//Notchpass filter.
		float temp;
		for(i = 0; i < size; i++) {
			temp = 1.0f - crealf(out[i]);
			out[i] = build_complex(temp, 0);
		}
	}

	return out;
}



///////////////////////////////////////////////////////////////////////////////

void dft2d(unsigned char* input, float* real, float* imag, int M, int N)
{
	int k, l, m, n, index, inter;
	float angle;

	for (k = 0; k < M; k++) {
		for (l = 0; l < N; l++) {
			index = k * N + l;
			real[index] = 0;
			imag[index] = 0;
			for (m = 0; m < M; m++) {
				for (n = 0; n < N; n++) {
					inter = m * N + n;
					angle = 2 * M_PI * (k * m / (float)M + l * n / (float)N);
					real[index] += input[inter] * cos(angle);
					imag[index] -= input[inter] * sin(angle);
				}
			}
		}
	}
}

#define N 4

void test_dft2(void)
{
	int m = N;
	int n = N;
	int i, j;

	unsigned char input[N][N];
	float real[N][N];
	float imag[N][N];

	// Initialize input signal
	printf("test dft2, %d lines, %d col\n", m, n);
	printf("original data\n");
	srand(1792); 
	for (i = 0; i < m; i++) {
		for (j = 0; j < n; j++) {
			input[i][j] = rand();
			printf("%.2d ", input[i][j]);
		}
		printf("\n");
	}

	dft2d((unsigned char*)input, (float*)real, (float*)imag, m, n);
	printf("after DFT: %d x %d \n", m, n);
	for (i = 0; i < m; i++) {
		printf("line %d: ", i);
		for (j = 0; j < n; j++) {
			printf("%5d:%5d ", (int)real[i][j], (int)imag[i][j]);
		}
		printf("\n");
	}
}

#include <string.h>
void test_2d_r2c(void) 
{
	int n0 = 8;
	int n1 = 8;
	int n1_cplx = n1 / 2 + 1;

	float *in;
	fftwf_complex *out;
	fftwf_plan p = fft_init(n0, n1, &in, &out);

	int i, j, index;
	// Initialize input signal
	printf("%d lines, %d col\n", n0, n1);
	printf("original data\n");
    for (i = 0; i < n0; i++) {
        for (j = 0; j < n1; j++) {
			index = i * n1 + j;
			if ((i + j) & 1)
				in[index] = -index;
			else
				in[index] = index;
			printf("%.3d ", (int)in[index]);
        }
		printf("\n");
    }

	fft_r2c(p, in, out);

	//print output
	printf("fftw r2c output\n");
	for (i = 0; i < n0; i++) {
		printf("line %d\n", i);
		for (j = 0; j < n1_cplx; j++) {
			index = i * n1_cplx + j;
			printf("%d: Real: %f, Imaginary: %f\n", j,\
				crealf(out[index]), cimagf(out[index]));
		}
		printf("\n");
	}

	//IFFT
	ifft_c2r(p, out, in);
	printf("2D IFFT output\n");
	int div = n0 * n1;
    for (i = 0; i < n0; i++) {
        for (j = 0; j < n1; j++) {
			index = i * n1 + j;
			printf("%.3d ", (int)in[index] / div);
        }
		printf("\n");
    }

	fft_exit(p);
}

void test_2d_c2c(void)
{
	int w = 8;
	int h = 6;

	fftwf_complex *in = fftwf_alloc_complex(w * h);
	fftwf_complex *out = fftwf_alloc_complex(w * h);

	int i, j, index;
	// Initialize input signal
	printf("c2c test: %d lines, %d col\n", w, h);
	printf("original data\n");
    for (i = 0; i < h; i++) {
        for (j = 0; j < w; j++) {
			index = i * w + j;
			in[index] = build_complex(index, index);
			printf("%.3d+%.3di ", index, index);
        }
		printf("\n");
    }

	fft_c2c(in, out, h, w);
	//print output
	printf("fftw c2c output\n");
	for (i = 0; i < h; i++) {
		printf("line %d\n", i);
		for (j = 0; j < w; j++) {
			index = i * w + j;
			printf("%d: Real: %f, Imaginary: %f\n", j,\
				crealf(out[index]), cimagf(out[index]));
		}
		printf("\n");
	}

	//IFFT
	ifft_c2c(out, in, h, w);
	printf("2D IFFT c2c output\n");
	int div = w * h;
	float a, b;
    for (i = 0; i < h; i++) {
        for (j = 0; j < w; j++) {
			index = i * w + j;
			a = crealf(in[index]) / div;
			b = cimagf(in[index]) / div;
			printf("%.3d+%.3di ", (int)a, (int)b);
        }
		printf("\n");
    }

	fftwf_free(in);
	fftwf_free(out);
}

